﻿using EasyFlow.Motion;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;

namespace Kimd.DMC3800Card
{
    public class DMC3800CardAxisOperator : IAxis
    {
        private bool _isErr;
        private string _lastError;
        private short currentCardNo = 0;
        public bool[] IsHomeFinish { get; set; } = new bool[8];
        public string LastError 
        {
            get { return _lastError; }
        }

        public bool IsError{get;set;}

        public double AccUnitConvert(double vel, double acc)
        {
            return acc;
        }
        public bool AxisGoHome(short cardNo, short axisNo, short homeMode, bool homeDir, AxisSpeed homeSpeed)
        {
            if (_isErr)
                return false;
            int rtn = 0;
            int dir = homeDir ? 1 : 0;//0 --反 1--正
            IsHomeFinish[axisNo - 1] = false;
            rtn += csLTDMC.LTDMC.dmc_set_homemode((ushort)cardNo, (ushort)(axisNo - 1), (ushort)dir, 0, (ushort)homeMode, 0); // 2
            if (!CheckApiResult(csLTDMC.LTDMC.dmc_set_profile((ushort)cardNo, (ushort)(axisNo - 1), homeSpeed.VelStart, homeSpeed.Vel, homeSpeed.Acc, homeSpeed.Dec, homeSpeed.VelEnd), "设置轴运动速度失败!"))
                return false;
            //if (!CheckApiResult(csLTDMC.LTDMC.dmc_set_el_home((ushort)cardNo, (ushort)(axisNo - 1), 1), "设置回零反找模式失败!"))
            //    return false;
            rtn += csLTDMC.LTDMC.dmc_set_home_el_return((ushort)cardNo, (ushort)(axisNo - 1), 1);  //设定遇极限反转
            rtn += csLTDMC.LTDMC.dmc_set_home_position((ushort)cardNo, (ushort)(axisNo - 1), 1, 0);  //设定回零完成位置清零
            rtn += csLTDMC.LTDMC.dmc_home_move((ushort)cardNo, (ushort)(axisNo - 1));  //开始回零

            currentCardNo = cardNo;
            HomeParam homeParam = new HomeParam()
            {
                AxisNo = axisNo,
            };
            Thread homeThread = new Thread(new ParameterizedThreadStart(home));
            homeThread.Start(homeParam);
            return true;
        }
        private void home(object no)
        {
            HomeParam homeParam = (HomeParam)no;
            int rtn = 0;
            while (true)
            {
                rtn = csLTDMC.LTDMC.dmc_check_done((ushort)currentCardNo, (ushort)(homeParam.AxisNo - 1));
                if (rtn == 1)
                {
                    Console.WriteLine($"轴{homeParam.AxisNo}: finish");
                    StopType st = GetStopReason((short)currentCardNo, (short)(homeParam.AxisNo - 1));
                    Console.WriteLine(st.ToString());
                    if (st == StopType.Normal)
                    {
                        /*回零成功*/
                        IsHomeFinish[homeParam.AxisNo - 1] = true;
                        Thread.Sleep(200);
                        rtn = csLTDMC.LTDMC.dmc_set_encoder((ushort)currentCardNo, (ushort)(homeParam.AxisNo - 1), 0);
                        rtn = csLTDMC.LTDMC.dmc_set_position((ushort)currentCardNo, (ushort)(homeParam.AxisNo - 1), 0);
                    }
                    else
                        IsHomeFinish[homeParam.AxisNo - 1] = false;
                    // rtn = csLTDMC.LTDMC.dmc_set_home_position((ushort)currentCardNo, (ushort)(axisno - 1), 0, 0);
                    break;
                }
                else
                {
                    //if (GetAxisStatus(currentCardNo, axisno).ServoWarn)
                    //{
                    //    HomeFound = false;
                    //    break;
                    //}
                }
                Thread.Sleep(2);
            }
        }
        public bool AxisMoveAbsolute(short cardNo, short axisNo, int distanceInPulse, AxisSpeed axisSpeed)
        {
            if (_isErr)
                return false;
            short rtn = csLTDMC.LTDMC.dmc_set_profile((ushort)cardNo, (ushort)(axisNo - 1), axisSpeed.VelStart, axisSpeed.Vel, axisSpeed.Acc, axisSpeed.Dec, axisSpeed.VelEnd);
            if (!CheckApiResult(rtn, "设置轴运动速度失败!"))
                return false;
            rtn += csLTDMC.LTDMC.dmc_pmove((ushort)cardNo, (ushort)(axisNo - 1), distanceInPulse, 1);
            return CheckApiResult(rtn, "设置轴点位运动失败!");
        }

        public bool AxisMoveContinous(short cardNo, short axisNo, MoveDirection direction, AxisSpeed axisSpeed)
        {
            if (_isErr)
                return false;
            short rtn = csLTDMC.LTDMC.dmc_set_profile((ushort)cardNo, (ushort)(axisNo - 1), axisSpeed.VelStart, axisSpeed.Vel, axisSpeed.Acc, axisSpeed.Dec, axisSpeed.VelEnd);
            if (!CheckApiResult(rtn, "设置轴运动速度失败!"))
                return false;
            return CheckApiResult(csLTDMC.LTDMC.dmc_vmove((ushort)cardNo, (ushort)(axisNo - 1), (ushort)(direction == MoveDirection.Positive ? 1 : 0)), "设置轴连续运动失败!");
        }

        public bool AxisMoveRelative(short cardNo, short axisNo, int offsetInPulse, AxisSpeed axisSpeed)
        {
            throw new NotImplementedException();
        }

        public bool CheckApiResult(short rtn, string err)
        {
            if (rtn != 0)
            {
                _isErr = true;
                _lastError = err;
                //string errorStr = ErrorCode2ErrorStr(rtn);
                //MachineMessage.CallException(ExceptionType.FatalErr, "轴卡", $"[{rtn}{errorStr}]:{err}", MachineErrCode.CardErr);
                return false;
            }
            return true;
        }

        public void ClearAxisStatus(short cardNo, short axisNo)
        {

        }

        public double GetAxisCommandPosition(short cardNo, short axisNo)
        {
            if (_isErr)
                return 0;
            return csLTDMC.LTDMC.dmc_get_position((ushort)cardNo, (ushort)(axisNo - 1));
        }

        public double GetAxisEncoderPosition(short cardNo, short axisNo)
        {
            if (_isErr)
                return 0;
            return csLTDMC.LTDMC.dmc_get_encoder((ushort)cardNo, (ushort)(axisNo - 1));
        }

        public AxisStatus GetAxisStatus(short cardNo, short axisNo)
        {
            if (_isErr)
                return new AxisStatus();
            uint sts = csLTDMC.LTDMC.dmc_axis_io_status((ushort)cardNo, (ushort)(axisNo - 1));
            //0 为 true 1为false
            AxisStatus axisSts = new AxisStatus()
            {
                ServoWarn = (sts >> 0 & 0x01) == 1,
                ReachPosition = (sts >> 8 & 0x01) == 1,
                NegativeLimit = (sts >> 2 & 0x01) == 1,
                PositiveLimit = (sts >> 1 & 0x01) == 1,
                Origin = ((sts >> 4) & 0x01) == 1,
                EmergencyStop = ((sts >> 3) & 0x01) == 1,
                Ready = csLTDMC.LTDMC.dmc_check_done((ushort)cardNo, (ushort)(axisNo - 1)) == 1,
                ServoOn = csLTDMC.LTDMC.dmc_read_sevon_pin((ushort)cardNo, (ushort)(axisNo - 1)) == 0,
            };
            // ushort homeSts = 0;
            //CheckApiResult(csLTDMC.LTDMC.dmc_get_home_result((ushort)cardNo, (ushort)(axisNo - 1), ref homeSts), "获取轴回零状态失败!");
            //axisSts.Home = homeSts == 1;
            return axisSts;
        }

        public StopType GetStopReason(short cardNo, short axisNo)
        {
            if (_isErr)
                return StopType.Alarm;
            int reason = 0;
            short rtn = csLTDMC.LTDMC.dmc_get_stop_reason((ushort)cardNo, (ushort)(axisNo), ref reason);
            switch (reason)
            {
                case 0:
                    return StopType.Normal;
                case 1:
                case 2:
                    return StopType.Alarm;
                case 4:
                    return StopType.Emg;
                case 5:
                case 6:
                case 7:
                case 8:
                    return StopType.HLimit;
                case 9:
                case 10:
                case 11:
                case 12:
                    return StopType.SLimit;
                case 13:
                case 14:
                    return StopType.Cmd;
                case 15:
                case 16:
                case 17:
                case 18:
                    return StopType.Other;
                case 19:
                    return StopType.IO;
            }
            return StopType.Other;
        }

        public void ResetServoWarn(short cardNo, short axisNo)
        {
            if (_isErr)
                return;
            short rtn = csLTDMC.LTDMC.dmc_write_erc_pin((ushort)cardNo, (ushort)(axisNo - 1), 0);
            Thread.Sleep(500);
            rtn += csLTDMC.LTDMC.dmc_write_erc_pin((ushort)cardNo, (ushort)(axisNo - 1), 1);
            CheckApiResult(rtn, "清除轴伺服报警失败!");
        }

        public void SetAxisServoOff(short cardNo, short axisNo)
        {
            if (_isErr)
                return;
            CheckApiResult(csLTDMC.LTDMC.dmc_write_sevon_pin((ushort)cardNo, (ushort)(axisNo - 1), 1), "设置轴伺服Off失败!");
        }

        public void SetAxisServoOn(short cardNo, short axisNo)
        {
            if (_isErr)
                return;
            CheckApiResult(csLTDMC.LTDMC.dmc_write_sevon_pin((ushort)cardNo, (ushort)(axisNo - 1), 0), "设置轴伺服ON失败!");
        }

        public void StopAxis(short cardNo, short axisNo, bool now)
        {
            if (_isErr)
                return;
            short rtn = 0;
            if (!now)
            {
                rtn = csLTDMC.LTDMC.dmc_set_dec_stop_time((ushort)cardNo, (ushort)(axisNo - 1), 0.1);
                CheckApiResult(rtn, "设置减速停止减速度失败!");
            }
            CheckApiResult(csLTDMC.LTDMC.dmc_stop((ushort)cardNo, (ushort)(axisNo - 1), now ? (ushort)1 : (ushort)0), "停止轴运动失败!");
        }

        public double VelUnitConvert(double vel)
        {
            return vel;
        }

        public void ZeroAxisCommand(short cardNo, short axisNo)
        {
            if (_isErr)
                return;
            CheckApiResult(csLTDMC.LTDMC.dmc_set_position((ushort)cardNo, (ushort)(axisNo - 1), 0), "清除命令位置失败!");
        }

        public void ZeroAxisEncoder(short cardNo, short axisNo)
        {
            if (_isErr)
                return;
            CheckApiResult(csLTDMC.LTDMC.dmc_set_encoder((ushort)cardNo, (ushort)(axisNo - 1), 0), "清除编码器位置失败!");
        }

        public bool GetAxisHomeFound(short cardNo, short axisNo)
        {
            if (_isErr)
                return false;
            return IsHomeFinish[axisNo-1];
        }

        public bool ClearOffset(short cardNo, short axisNo)
        {
            throw new NotImplementedException();
        }

        public bool AxisChangeSpeed(short cardNo, short axisNo, AxisSpeed changedSpeed)
        {
            throw new NotImplementedException();
        }

        public void ConfigureCompareOut(short cardNo, short axisNo, short outPort, short logic, int pulseWidth, List<int> comparePulse)
        {
            throw new NotImplementedException();
        }
    }

    public class HomeParam
    {
        public short AxisNo { get; set; }
        public bool IsHomeFinish { get; set; }
    }
}
